﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using BaseDll;
using Communicate;
using log4net;
using SerialDict;
using MotionIoLib;
using System.Threading;
using UserData;
using System.ComponentModel;
using System.Net;
using System.Net.Sockets;
namespace OtherDevice
{
    [Description("埃斯顿")]
    public class Robot_埃斯顿:SixAxisRobot_Base
    {
        public Robot_埃斯顿()
        {
            controlFrom = new Form_SixAxisRobot();
        }
        private Socket socketCommunication;
        private string clientIP = "192.168.6.69";//机器人ip  192.168.6.69
        private Dictionary<string, Socket> dict = new Dictionary<string, Socket>();
        private Dictionary<string, Thread> dictThread = new Dictionary<string, Thread>();
        bool b_IsConnect = false;
        public override int linkEtherNetControl()
        {
            try
            {
                Socket socketWatch = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                IPAddress ip = IPAddress.Parse(paramter.str_IP地址);
                IPEndPoint endPoint = new IPEndPoint(ip, paramter.n_Port);
                socketWatch.Bind(endPoint);
                socketWatch.Listen(10);
                Thread thread = new Thread(Listen);
                thread.IsBackground = true;
                thread.Start(socketWatch);
                return 0;
            }
            catch (Exception ex)
            {
                return -1;
            }

        }
        void Listen(object obj)
        {
            Socket socketWatch = obj as Socket;
            while (true)
            {
                try
                {
                    //等待客户端的连接，同时会创建一个与其通信的socket
                    socketCommunication = socketWatch.Accept();
                    if (!dict.Keys.Contains(socketCommunication.RemoteEndPoint.ToString().Split(':')[0]))
                    {
                        dict.Add(socketCommunication.RemoteEndPoint.ToString().Split(':')[0], socketCommunication);
                        if(dict.ContainsKey(clientIP))
                           b_IsConnect = true;
                    }
                    else
                    {
                        dict[socketCommunication.RemoteEndPoint.ToString().Split(':')[0]] = socketCommunication;
                    }
                    Thread thread = new Thread(RecMsg);
                    thread.IsBackground = true;
                    thread.Start(socketCommunication);
                    dictThread.Add(socketCommunication.RemoteEndPoint.ToString().Split(':')[0], thread); // 将新建的线程 添加 到线程的集合中去。
                }


                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message);
                }
            }
        }

        void RecMsg(object sokConnectionparn)
        {
            Socket sokClient = sokConnectionparn as Socket;
            while (true)
            {
                // 定义一个2M的缓存区；
                // byte[] arrMsgRec = new byte[1024 * 1024 * 2];
                byte[] arrMsgRec = new byte[1024];
                // 将接受到的数据存入到输入 arrMsgRec中；
                int length = -1;
                try
                {
                    length = sokClient.Receive(arrMsgRec); // 接收数据，并返回数据的长度；
                    if (length == 0)
                    {
                        // 从 通信套接字 集合中删除被中断连接的通信套接字；
                        dict.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                        // 从通信线程集合中删除被中断连接的通信线程对象；
                        dictThread.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                        sokClient.Shutdown(SocketShutdown.Both);
                        sokClient.Close();
                        return;
                    }
                }
                catch (Exception e)
                {
                    // 从通信套接字 集合中删除被中断连接的通信套接字；
                    dict.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                    // 从通信线程集合中删除被中断连接的通信线程对象；
                    dictThread.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                    break;
                }
                //获取接收的消息
                string ReciveMsg = Encoding.UTF8.GetString(arrMsgRec, 0, length).Split('\r', '\n')[0];// 将接受到的字节数据转化成字符串；
                InMessageQueue(ReciveMsg);
            }
        }

        public void Send(string msg)
        {
            if (dict != null && dict.Count > 0)
            {
                Socket socket = dict[clientIP];
                if (socket != null)
                {
                    byte[] buffer = Encoding.UTF8.GetBytes(msg);
                    socket.Send(buffer);
                }
            }
        }


        //设置负载
        public override bool SetPayload(int Scope, string payloadName)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetPayload_IFace(" + Scope.ToString() + ",\"" + payloadName + "\"); id=35]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }
        //切换成大地坐标系
        public override bool SetJogCoordType(int Scope)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetCoordType_IFace(" + Scope.ToString() + "); id = 19]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }
        }
        //切换成工具坐标系
        public override bool SetToolCoord(int Scope, string ToolCoordName)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetTool_IFace(" + Scope.ToString() + ",\"" + ToolCoordName + "\"); id=20]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }
        }
        //切换成用户坐标系
        public override bool SetUserCoord(int Scope, string UserCoordName)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetCoord_IFace(" + Scope.ToString() + ",\"" + UserCoordName + "\"); id=21]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }
        }

        public override bool ReadDigitalInPort(int ioIndex)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[IOGetDin(" + ioIndex.ToString() + "); id = 1]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("[id = 1; Ok; 1]"))
                {
                    return true;
                }
                else if (n_Result == 0 && strMeg.Contains("[id = 1; Ok; 0]"))
                {
                    return false;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }
        public override bool SetDigitalOutputPort(int ioIndex, bool b_On)
        {
            try
            {
                string strOn = b_On ? "1" : "0";
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetIOValue(" + ioIndex.ToString() + ",1" + "," + strOn + "); id = 1]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }

        //获取当前位置
        public override PointWorld GetCurrentPos_World()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[GetCurWPos();id=2]", ref strMeg);
                PointWorld PointWorldObject = new PointWorld(0, 0, 0, 0, 0, 0);
                if (strMeg == null)
                    return PointWorldObject;
                if (strMeg.Contains("Ok"))
                {
                    try
                    {
                        string CurrentPos_Joint = strMeg;
                        int StartPosition = CurrentPos_Joint.IndexOf("Ok") + 4;
                        int EndPosition = CurrentPos_Joint.IndexOf("]");
                        string SubCurrentPos_Joint = CurrentPos_Joint.Substring(StartPosition, EndPosition - StartPosition);
                        string[] SubCurrentPos_JointArray = SubCurrentPos_Joint.Split(new char[] { ' ' });
                        PointWorldObject.X = double.Parse(SubCurrentPos_JointArray[0]);
                        PointWorldObject.Y = double.Parse(SubCurrentPos_JointArray[1]);
                        PointWorldObject.Z = double.Parse(SubCurrentPos_JointArray[2]);
                        PointWorldObject.W = double.Parse(SubCurrentPos_JointArray[3]);
                        PointWorldObject.P = double.Parse(SubCurrentPos_JointArray[4]);
                        PointWorldObject.R = double.Parse(SubCurrentPos_JointArray[5]);
                    }
                    catch (Exception ex)
                    {
                        PointWorldObject.X = 0;
                        PointWorldObject.Y = 0;
                        PointWorldObject.Z = 0;
                        PointWorldObject.W = 0;
                        PointWorldObject.P = 0;
                        PointWorldObject.R = 0;
                    }
                }
                else
                {
                    PointWorldObject.X = 0;
                    PointWorldObject.Y = 0;
                    PointWorldObject.Z = 0;
                    PointWorldObject.W = 0;
                    PointWorldObject.P = 0;
                    PointWorldObject.R = 0;
                }

                return PointWorldObject;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}");
                return new PointWorld(0, 0, 0, 0, 0, 0); ;
            }


        }
        //直线运动
        public override bool MoveLine(PointWorld Point, int Speed = 100, int BlendValue = 500)
        {
            string Commond = "[MoveToSelectPointb_IFace(2," + "\"" + Point.X.ToString() + "_" + Point.Y.ToString() + "_" + Point.Z.ToString() + "_" + Point.W.ToString() + "_" + Point.P.ToString() + "_" + Point.R.ToString() + "\"," + "\"" + "0" + "_" + Speed.ToString() + "_" + BlendValue.ToString() + "\"" + "); id = 16]";
            int n_Result = 0;
            string strMeg = "";
            n_Result = Function_SendCmd(Commond, ref strMeg);
            if (n_Result == 0 && strMeg.Contains("Ok"))
            {
                return Function_IsInpos(Point);
            }
            else
            {
                return false;
            }

        }
        //关节运动--未开发成功
        public override bool MoveJog(PointWorld Point, int Speed = 100, int BlendValue = 500)
        {
            string Commond = "[MoveToSelectPointb_IFace(1," + "\"" + Point.X.ToString() + "_" + Point.Y.ToString() + "_" + Point.Z.ToString() + "_" + Point.W.ToString() + "_" + Point.P.ToString() + "_" + Point.R.ToString() + "\"," + "\"" + "0" + "_" + Speed.ToString() + "_" + BlendValue.ToString() + "\"" + "); id = 16]";
            int n_Result = 0;
            string strMeg = "";
            n_Result = Function_SendCmd(Commond, ref strMeg);
            if (n_Result == 0 && strMeg.Contains("Ok"))
            {
                return Function_IsInpos(Point);
            }
            else
            {
                return false;
            }
        }

        public override int ChangeRunMode(int modeType)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[changemode_IFace({modeType}); id = 9]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 9; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }

        }
        public override int ReadRunMode()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[getCurSysMode_IFace(); id = 10]", ref strMeg);
                if (strMeg == "[id = 10; Ok; 0]")
                {
                    return 0;
                }
                else if (strMeg == "[id = 10; Ok; 1]")
                {
                    return 1;
                }
                else if (strMeg == "[id = 10; Ok; 2]")
                {
                    return 2;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int SetServo(bool b_Statue)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                int nValue = b_Statue ? 1 : 0;
                n_Result = Function_SendCmd($"[SetMotServoStatus_IFace({nValue}); id = 23]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 23; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override bool ReadServoStatue()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[GetServoSts_IFace(); id = 24]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 24; Ok; 1]")
                {
                    return true;
                }
                else
                {
                    return false;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;

            }

        }
        public override int ReadRunState()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[getRobotRunStatus_IFace(); id = 13]", ref strMeg);
                if (strMeg == "[id = 13; Ok; 0]")
                {
                    return 0;
                }
                else if (strMeg == "[id = 13; Ok; 1]")
                {
                    return 1;
                }
                else if (strMeg == "[id = 13; Ok; 2]")
                {
                    return 2;
                }
                else if (strMeg == "[id = 13; Ok; 3]")
                {
                    return 3;
                }
                else if (strMeg == "[id = 13; Ok; 4]")
                {
                    return 4;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int ResetAlarm()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[resetErrorId_IFace(); id = 14]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 14; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int RunStart()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[startRun_IFace(); id = 16]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 16; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int RunStop()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[stopRun_IFace(); id = 17]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 17; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int ReadRunSpeed()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[getGoableSpeed_IFace(); id = 29]", ref strMeg);
                string Speed = strMeg.Replace("id = 29; Ok; ", "").Replace("[", "").Replace("]", "");

                if (n_Result == 0 && strMeg != "[id = 122; FAIL]" && Speed != "")
                {
                    return int.Parse(Speed);
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int SetRunSpeed(int Speed)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[setGoableSpeed_IFace({Speed}); id = 11]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 11; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override bool IsConnect()
        {
            try
            {
                return b_IsConnect;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}");return false;
            }
        }
        object obj_Lock = new object();
        private int Function_SendCmd(string strCmd,ref string strResult)
        {
            lock (obj_Lock)
            {
                this.Send(strCmd);
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Restart();
                while (true)
                {
                    Thread.Sleep(10);
                    string mes;
                    int ret = OutMessageQueue(out mes);
                    if (ret == 0)
                    {
                        strResult = mes;
                        return 0;
                    }
                    else if (stopwatch.ElapsedMilliseconds > 2000)
                    {
                        return -4;
                    }
                }


            }
        }

        private bool IsInpos()
        {
            string Commond = "[IsRobotMoving_IFace(); id = 21]";
            int n_Result = 0;
            string strMeg = "";
            n_Result = Function_SendCmd(Commond, ref strMeg);
            if (n_Result == 0 && strMeg== "[id = 21; Ok; 0]")
            {
                return true;
            }
            else
            {
                return false;
            }

        }


        private bool Function_IsInpos(PointWorld Point)
        {
            System.Diagnostics.Stopwatch StopwatchObject = new System.Diagnostics.Stopwatch();
            StopwatchObject.Start();
        CurrentPosLabel:
            if (GlobalVariable.g_StationState == StationState.StationStatePause)
                goto CurrentPosLabel;
            if (IsInpos())
                return true;
            else
            {
                TimeSpan TimeSpanObject = StopwatchObject.Elapsed;
                if (TimeSpanObject.Seconds < 20)
                {
                    goto CurrentPosLabel;
                }
                else
                    return false;
            }
        }

    }
}